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I. INTRODUCTION 


A. BACKGROUND 


The number of Third World/non-super power countries exercising their armed 
forces is increasing as border disputes, religious difference, and etc. fuel aggression. 
Although a majority of these nations have no ill intentions towards the United States, 
they may pose a very dangerous threat to U.S. vessels. By using minefields, a nation can 
stifle the use of a waterway. During the Persian Gulf War, mine fields were laid in the 
Persian Gulf by Iraqi forces. As a result of the mine layings, sea-lanes were severely 
restricted until proper mine clearing of frequented water ways could be completed. To 
clear minefields the Navy currently uses specialized ships, aircraft, and personnel. In any 
of the above methods, there is a very high risk of human life loss in the event of an 
accident. As a result, alternate means of conducting mine clearing tasks have been 
explored including the use of marine mammals and Autonomous Underwater Vehicles 
(AUVs) /Unmanned Underwater Vehicles (UUVs). Marine mammal systems have the 
drawbacks of limited numbers, and extensive, expensive, training requirements. 
Additionally, their use would likely cause opposition from the public. Fortunately, 
AUVs are designed to be low maintenance, low cost, ‘expendable’ vehicles designed to 
carry out the mission in its program. 

AUVs and UUVs are rapidly finding increased potential in military applications. 
From deep-water salvage to mine hunting/clearing operations, this new breed of vehicles 
provides a means for dangerous missions to be carried out without endangering 


personnel. More specifically, use of low cost AUVs in mine hunting/clearing operations 


appears specifically attractive since there is no risk of human life loss or damaging 
expensive ships. 

It is proposed that teams of AUVs working together may be used to search and 
identify large minefields. Such a team would consist of several “worker” vehicles and 
one “master” vehicle. The “worker” vehicles are expendable, low cost, AUVs designed 
to locate and possibly detonate enemy mines. The “master” vehicles are better equipped 
with more complex navigation, sonar, and communication suites. Pre-programmed 
"workers" would carry out their specific missions while the "master" vehicle would track 
and direct the drones to their specific waypoints. For a “master” vehicle to track several 
"workers", problems arise for a small, weight-conscious, AUV. Conventional position 
estimation in underwater applications usually consists of either active or passive towed 
array sonar. Unfortunately each application has drawbacks that preclude their use 
onboard the AUV. Active sonar would provide the most up to date and accurate 
positions of the worker vehicles. However, the transducer required to provide accurate 
positions of all the workers would be too large, require too much power, and be cost 
prohibitive for the size required in an AUV. Passive sonar is not a good choice for the 
AUV due to the time-late properties of its information. Additionally, vehicle mounted 
listening equipment that is of acceptable size, quality, and range is cost prohibitive, and 
use of a passive array requires handling equipment, which is not conducive to use in an 
AUV. Both passive and active tracking methods lack a means to explicitly discern mine- 
like objects from friendly AUVs. 

In an effort to conserve space and weight, a means of Range only Measurement 


(ROM) position estimation is proposed. Song (1999) has demonstrated the observability 


of ROM target tracking through use of simulations. Use of ROM tracking combines 
aspects of conventional active and passive tracking techniques. Like an active sensor, 
sound signals are emitted into the water using space and weight conscious acoustic 
modems transmit data through the water among vehicles in the team. Each signal 
contains "sent time", course, and identification information. This information enables the 
“master” vehicle to estimate the distance between sender and receiver, and to identify 
each sender. Upon receiving the signal from the "master" vehicle, the drone vehicle 
transmits its signal to "master" vehicle. When the "master" vehicle receives this signal, 
the distance between the two vehicles can be estimated by calculating time of travel for 
the signal. To ensure maximum accuracy, the "master" vehicle would use water property 


values attained from ‘now’ data. 


B. SCOPE OF THIS WORK 


The overall problem of ROM position estimation is complex and diverse since it 
is usual that either range and bearing, or two range values are needed for position 
estimation. This study includes the development of the dynamic model and an Extended 
Kalman Filter (EKF) to demonstrate the accuracy and viability of ROM techniques for 
position estimation. The purpose of this thesis is twofold: 

1. To prove the observability of ROM tracking techniques. 


2. To illustrate the accuracy of ROM tracking by comparing it to dead reckoning, 
a common means of simple navigation. 


Chapter II will provide the background of the Naval Postgraduate School (NPS) 
Acoustic Reconnaissance Interactive Exploratory Server (ARIES) AUV including 


navigation and sensor capabilities that are the basis of the “master” AUV’s capabilities. 


Chapter III explains the theory and modeling of the simulation with detailed descriptions 
of the construction of the Extended Kalman Filter (EKF), and the means of determining 
the system's observability. Chapter IV summarizes the results of the simulations. 
Chapter V presents the conclusions gathered from the results, and offers 


recommendations for continued research on this topic. 


П. OVERVIEW OF ARIES 


A brief overview of the Naval Postgraduate School ARIES AUV shall provide 
some the working knowledge necessary to understand why certain assumptions were 
made, and to familiarize the reader with the AUV. 

ARIES weighs 225 kilograms and measures about three m long, 0.4 m wide and 
0.25 m high. The hull is constructed of one-quarter inch thick aluminum and forms the 
main pressure vessel that houses all electronics, computers, and batteries. External 
sensors are housed in the flooded fiberglass nose. Figure 2.1 shows the ARIES 
component layout. The vehicle has a top speed of three and one-half knots, which it may 
maintain for approximately four hours. The ARIES was primarily designed for shallow 


water operations and can operate safely down to 30 meters (Marco, Healey, 2000). 


A. NAVIGATION 


The sensor suite used for navigation includes a RD Instruments Navigator 
Doppler Velocity Log (DVL) that also contains a magnetic compass. This instrument 
measures the vehicle ground speed, altitude, and magnetic heading. Angular rates and 
accelerations are measured using a 3-axis Motion Pak Inertial Motion Unit (IMU). Data 
from the DVL and IMU are fused using an EKF to provide a high quality dead reckoning 
solution while submerged. While surfaced, ARIES utilizes Differential Global 
Positioning System (DGPS) to correct any navigational errors accumulated during the 
submerged phases of a mission. 

The EKF in the ARIES’ navigational suite may be tuned for optimal performance 


given a set of data. By fusing data input from the IMU / DVL / DGPS suite, biases, such 


as yaw rate bias and compass bias, can be identified and compensated for during 
underwater travel. Although this compensation can not completely correct for 
environmental biases, a relatively short surface time, for example, 10 seconds allows the 
filter to re-estimate biases, correct position estimates and continue with improved 
accuracy (Marco, Healey, 2000). 

For obstacle avoidance and target acquisition, the ARIES uses scanning or 
profiling sonar. The sonar heads can scan continuously through 360° of rotation or 


sweep through a defined angular sector (Marco, Healey, 2000). 


B. COMMUNICATIONS 


ARIES is equipped with two types of modems. Radio modems are used for high 
bandwidth command/control and и СМ, while the vehicle is surfaced. An 
acoustic modem is used for low bandwidth communications while the vehicle is 
submerged. Additionally, the acoustic modem enables ARIES to exchange pertinent data 


with other AUVs that have compatible components (Marco, Healey, 2000). 


С. SERVER VEHICLE CONCEPT 


This thesis models the “master” vehicle the server of a two vehicle network. 
Proposals for using the NPS ARIES as a network server vehicle for multi-vehicle 
cooperative operations have surfaced due to the foreseen benefits. Use of the server 
vehicle as a data relay increases the range of communications of the underwater 
components of the network. Figure 2.2 describes the concept where in position 1, the 


ARIES communicates through its acoustic modem with multiple worker vehicles that are 


engaged in a search pattern. Position 2 shows the ARIES on the surface using a radio 
modem to report mission status of the worker vehicles (possibly vehicle positions, image 
snippets of targets, and hydrographic data) to the command ship. While surfaced the 
server vehicle can receive tactical decision re-tasking commands. Once the new orders 
are received, the vehicle will submerge and transmit, using its acoustic modem, new tasks 
to each worker vehicle. Using a server vehicle eliminates the complexity of deploying 
fixed buoys. Also, a vehicle of this type can achieve close proximity or rendezvous with 
the worker vehicles allowing for higher acoustic bandwidth data transfer (Marco, Healey, 


2000). 
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Figure 2.2 - Sever vehicle concept. 1. Low bandwidth submerged data transfer 
between underwater vehicles. 2. High-speed data relay to command ship (Marco, 
Healey, 2000). 
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Ш. RANGE ONLY MEASUREMENT (ROM) THEORY 


A. INTRODUCTION 


Only in the past few years has serious research in Range Only Measurement 
(ROM) target tracking been conducted. Seen as a means to reduce the cost of active 
target tracking, methods of ROM target tracking are increasingly being investigated. 
ROM target tracking previously lacked interest of researchers due to “the lack of firm 
analytical basis for system observability in addition to the lack of proper filter structures 
in the field of target tracking with ROM from one observer.” (Song, 1999) 

Previous study also found that, “global system observability may not exist for the 
ROM target tracking; however, target state variables such as position, velocity, and 
acceleration can be estimated with а proper initial state estimate due to local system 
observability." (Song, 1999) If available, an initial state estimate can be obtained from 
previous target data, an update from an external source, i.e. a surface ship, or through the 
use of the “master” vehicle's active sonar. 

This study furthers the above by showing that accurate position estimates can be 
obtained without an accurate initial position, and that global observability for ROM target 


tracking is possible under certain conditions. 


B. PROBLEM FORMULATION 


To properly investigate the questions concerning ROM's accuracy and 


observability an appropriate model had to be constructed. Since this tracking technique 


may be used on an AUV, the NPS ARIES AUV provided the basis for model vehicles’ 
capabilities and constraints. 

Modeling of the ROM system was done in computer based simulations using 
MATLAB due to its strong processing and presentation capabilities. Two simulations 
were developed; each models the same problem with a few changed parameters. 
Development of the simulation models was split into two sections: 

1. The dynamic model 

2. The Extended Kalman Filter (EKF) 

The development and theory behind constructing the above sections will be explained in 


detail. 


1. The Dynamic Model 


The simulations model a system consisting of two AUVs, a “master” and a 
“drone” vehicle operating in a zero current environment. The depths of the two vehicles 
were disregarded as this is assumed to be only a two dimensional problem. The “master” 
AUV navigation suite matches that of the AIRES described in the previous chapter, and 
is assumed to have accurate position, velocity, acceleration, and course data at all times. 
On the contrary, the “drone” vehicle possesses a rudimentary navigation suite consisting 
of an off the shelf compass and a bottom-mounted Doppler Sonar Velocity Log. 
Combining its compass heading, speed over ground, and an initial estimated position, the 
drone uses dead reckoning to estimate and update its position. 

For both simulations, almost ideal surroundings were used to model the 


environment. As stated above, both simulations were modeled without current or other 
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external forces, i.e., wind, waves, etc. To introduce an error between the actual and the 
dead reckoning courses, the compass was assumed to have a constant bias of five degrees 
east. The bias may seem artificially large, but testing with the AIRES has proven that 
this value represents actual bias errors experienced using comparable compasses. Other 
environmental factors that would affect the vehicles’ motion were made negligible in 
order to focus on the details more critical to this thesis. 

To provide the best conditions for ROM observability, careful selection of vehicle 
paths was necessary especially that of the "master" vehicle. In previous study, Dr. Taek 
Song found that: 

...1f the tracker-target relative motion results in a constant bearing 
trajectory, or if the tracker is moving with a constant velocity or a constant 
acceleration, the system is not observable. This implies that the tracker 
motion should contain nonzero jerk to track a target with constant 
acceleration. (Song, 1998) 

Based on this observation, it was concluded that a circular path of some sort at maximum 
cruising speed would provide the best results by satisfying Dr. Song's criteria in tracking 
linear paths. It was decided to make the "master" /tracker vehicle's path a circle. In 
doing this, the “master” AUV's bearing, opening, and closing rates relative to the "drone" 
AUV could be altered by changing its path radius. In all cases, the "master" AUV's 
speed of one and one-half meters per second remained constant. 

A more straight forward task, the “drone”/target vehicle path selection represents 
an AUV conducting a portion of its minesweeping search pattern at a constant speed of 
two-tenths meters per second. For the Case One simulation, the “drone’s” dead 


reckoning and actual positions start in the center of the “master” vehicles circular path. 


The desired and dead reckoning course is due north, course 000, but due to the compass 
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error the actual path taken by the “drone” is five degrees east or course 005. Figure 3.1 
illustrates the three paths described above. The Case Two simulation assumes the actual 
initial position of the “дгопе” is unknown, but located near the circular path of the 
"master" vehicle. This was done using MATLAB's "randn" function which produces 
random entries chosen from a normal distribution with a zero mean and a variance of one. 
As in the Case One simulation, the actual drone model moves from its initial position on 
a course of 005 due to its compass bias. The "drone's" dead reckoning position starts in 
the center of the “master” vehicle’s path and follows the desired due north course. Figure 
3.2 shows the AUV paths in a typical Case Two simulation. 

For each simulation, a five element state vector in the Cartesian coordinates 
consisting of two “master” AUV position states, [X, Y], two “drone” AUV”s position 
states, [x;, уу], and the “drone” AUV”s heading state, [y], represented the system. The 


continuous dynamic system 1s expressed as 


x= Ax+ Bu 
where (A xy Y, 
00000 0 
-w 0 0 0 0 0 
А-т0 о 00 0 B 3 
0 0000 0.2 
0 0000 0 
and 
EJ 


Um 


in which Rs equals the radius in meters of the “master” AUV’s continuous circular path 
at speed Um. It was assumed that the tracking system dynamics are represented perfectly 


so that the process noise is not included in this analysis, (Song, 1999). 
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The next step in the problem formulation was converting the continuous dynamic 
system into a discrete dynamic system so it would be compatible with the EKF. 
MATLAB 's continuous to discrete function, "c2d", eased the process considerably. The 
basics of the “c2d” algorithms are taken from the Control Toolbox and described here for 
background on the process. 

To convert a state space linear time invariant (LTI) system expressed as 

х= Ах+ Ви 
into a discrete time state space system represented as 
ХаіəңҺ Ox, +I ty 


ИК =] 


MATLAB calculates the matrix exponential phi, (dt), as 
Ф- е4 
dt = the sample time increment 
and gamma, I’, as a value that maps inputs to system response such that 
ПЕСО АВ. 
With the values of phi and gamma, recursive loops in the simulation program calculate 


the discrete vehicle states for each increment of the specified simulation time length, f. 


on Kalman Filtering 

The Kalman filter is a set of mathematical equations that provides an efficient 
recursive solution of the least-squares method. The filter is very powerful in several 
aspects: 1t supports estimations of past, present, and even future states, and it can do so 


even when the precise nature of the modeled system is unknown (Bishop and Welch, 


15 


1995). The algorithm computes the best estimates of system variables arising from 
sensor based data and the system model. Data from measurements along with the 
measurement model are used in a system model to provide the least squares fit estimate 
of system state, based on those measurements (Healey, An, Marco 1998). The Kalman 
filter also assumes there is a zero mean error associated with the measurement data and 
estimation process. Based on a priori information, the modeler has the ability to adjust 
these error values to increase simulation accuracy. 

Due to the nonlinear nature of equations used in the simulations, an EKF was: 
used to fuse the available data and provide position estimates. The system is a 


continuous time model of vehicle motion represented by 


x(t) = f (s(t))+ q(t); 
y(t) = A(x(t)) + vo); 


where x(t)e 9t?" is the model state, f and h are continuous functions differentiable by 
x(t), and q and v are zero mean white noise excitations for the system and measurement 
models respectively (Healey, An, Marco, 1998). For the "master" AUV, the states are 
globally referenced longitude and latitude in meters, X and Y, the “drone” AUV states are 
also globally referenced, actual, longitude and latitude in meters, x; and y,;, and the 
heading angle referenced to North, yw (Stinespring, 2000). The filter state vector, x, is 
made up of the above states, and the system is of order five. 
к= у и ; 
The state model is related through the following set of functions representing 


dynamic relationships between states with assumptions embodying maneuvering models: 
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X =wY 


Ү = —oX 
ху = 0; 
У = 0; 
y =0; 


To calculate the above state functions and solve the equations, the EKF takes the 
outputs from some of the “master” AUV’s sensors measurements. The particular sensors 
that provide measurement data at that time are related to the filter state with the C matrix 
(Healey, An, Marco, 1998). This matrix is also used to convert estimated state vectors 
into the output matrix y. This matrix is in the form: 

y'=[Range, AUT YES] 
The equations, which are related in the C matrix and determine the output based on what 


the system is receiving from the sensors are as follows (Stinespring, 2000): 


ул = (X, За И D -y3,)° = Range 


y, = X; 
yy = Y; 
y4 =X; 
JS DATO 
Ye =V; 


The output y; is range between the “master” and “drone” vehicles at the time 
increment k, y2,3 are the “master” vehicle’s position components as recorded from DGPS, 
y4,5 are the “drone” position components as reported by their onboard position estimators, 
and yg is the “drone” AUV’s heading as reported from its compass. The output is then 
used to refine the drone position based on the sensor outputs. It was assumed that ranges 
calculated by the acoustic modem signal travel times would be accurate. Ranges used by 
the EKF in the simulations are exact and utilize data that would not be available to the 


filter in real world situations. 


I 


The C matrix composition is the only difference in the EKF between the Case 
One and Case Two simulations. In the Case One simulation, the C matrix's construction 
relates data from all sensors channels to EKF. This simulation provides more data than 
the EKF needs to provide an accurate position estimate, but it served as excellent 
program to tune the EKF. In the Case Two simulation, the C matrix only allows channels 
one through three, and six to be used by the EKF. The remaining channels were made 
unusable to the EKF by zeroing the applicable coefficients in the C matrix. 

As stated before, the simulations represent near ideal systems. The system and 
measurement noise matrices of Q and R, respectively, were kept relatively small and can 
be considered to have little to no effect on the results for both simulations, except to 


determine the response time of the EKF. 


S. Observability Determination 


EKF solution observability is probably the most important parameter in whether 
the data from the EKF is valid. Observability of the filter is essential in order to 
guarantee stable unbiased estimation errors of the state. A nonlinear filter such as the one 
used in the simulations, can be verified for observability locally through linearization of 
the f and h functions (Healey, An, Marco, 1998). For a linearized model, 


X(t) = Ax(t)+ g; 
y(t) = Сха) жу; 


where Есі апа Cm 
Ox Ox 


the system is locally observable if the following observability matrix, O, has full rank of 


five. 


ОТС А САС САО 
To calculate the total system observability, the integral of the observability grammian 
over the entire simulation time period, £, must be of full rank. 
гапК( |} (ФТ! (2,0)C! (t)C(t)@(1,0))dt) ) = n; 
where n=length(x); 
Observability matrices of full rank in each simulation provide an excellent basis for 


establishing ROM’s observability. 
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Figure 3.1 - AUV paths with known *'drone" initial position 
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Figure 3.2 - AUV paths with random, unknown “drone” initial position 


21 





THIS PAGE INTENTIONALLY LEFT BLANK 


22 





IV. RESULTS OF SIMULATIONS 


A. INTRODUCTION 


During development and data collection, several server vehicle speeds and 
circular path radıi were used. As a means to limit the amount of data collected, vehicle 
speeds were kept constant and turning capabilities were based on those of the AIRES 
AUV. 

Two simulation scenarios were modeled to test the ROM EKF. In the Case One 
scenario, the starting position of drone vehicle was constant and accurately passed to the 
EKF. Additionally, by modifying the C matrix, described in the previous chapter, the 
EKF used all the channels of measured sensor data to more accurately calculate the 
“drone” AUV’s position. In the Case Two scenario, it was intended to model a system in 
which the starting position of the drone AUV was unknown, and it's position data was 
not available to the EKF. 

To further characterize the accuracy of the EKF, the simulations were run with 
“master” AUV path radu of 17, 30, 50, and 100 hundred meters. The speed of the 
"master" vehicle was kept constant at one and one-half meters per second, which is the 


AIRES' maximum speed. 


B. CASE ONE MODEL 


In this scenario, the “drone” AUV’s actual and dead reckoning position started at 
the center “master” AUV’s circular path. As the simulation progresses, the desired and 


actual paths of the “drone” AUV diverge due to compass bias as shown in Figure 3.1. 
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The C matrix composition allows the actual position and heading states of the “drone” 


vehicle to pass to the EKF. 


]. Observability 


In all Case One simulations, the system exhibited complete observability. The 
system's local observability matrix and observability grammian were always of full rank. 
This was to be expected since all of the filter states were updated by sensor data at every 
time increment. Figures 4.1 and 4.2 depict the local and total system observability, 
respectfully. These figures are applicable to all “master” AUV path radii conducted in 


this scenario. 


2 КОМ Ассигасу 


Configured to relate data from all measurement channels, the EKF made accurate 
position approximations, but a noticeable oscillatory error in the east-west/cross track 
accuracy plagued runs with larger "master" AUV paths. Further testing determined that 
the error was related to the speed in which the “master” AUV completed its circular path 
or in other words its cyclic speed. Since the "master" AUV's speed was kept fixed, 
modifying its path radius was the only means to effect cyclic speed. Decreasing the 
radius of “master” AUV's path increased its cyclic rate. Accuracy of the EKF improved 
dramatically as the cyclic rates of the “master” AUV were increased. The difference in 
position between the actual and estimated “drone” AUV position drops considerably as 
illustrated in the following figures. Figure 4.3 compares the actual east-west position to 


the EKF estimated position for a "master" AUV path radius of 100 meters. Figure 4.4 
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compares the actual north-south position to the EKF estimated position for a "master" 
AUV path radius of 100 meters. Figures 4.5 through 4.10 show the same information as 
figures 4.4 and 4.5, for the three other “master” AUV paths. 

In comparing ROM position estimation to dead reckoning, ROM demonstrated 
itself as a superior. Unlike dead reckoning, which may produce unbounded position 
errors, ROM position estimation solutions converge if given sufficient time. For short 
periods of time, dead reckoning provided positions that rival the accuracy of ROM 
position estimation. However, for periods longer than approximately 1000 seconds, 
ROM position estimates mirror the “drone?” AUV's actual position much more closely 
than dead reckoning. 

Figure 4.11 shows the paths of the “master” AUV, the “drone” AUV’s desired 
and actual paths, and the ROM position estimate path. This figure clearly depicts ROM's 
accuracy over dead reckoning. To further show ROM’s accuracy over dead reckoning, 
Figure 4.12 illustrates the normalized differences between the dead reckoning and actual 
positions and the ROM estimated and actual positions. Figure 4.13 shows the error of 
ROM position estimation states is bounded, and over time decreases as it converges. 
Figures 4.14 through 4.22 illustrate the information displayed in Figures 4.11 through 
4.13 for the other “master” AUV path radii. These figures clearly show that as the 


“master” AUV’s cyclic rate increases, the accuracy of position estimation also increases. 


C. CASE TWO MODEL 


It was intended for this portion of the simulation to model a scenario in which the 


“master” AUV did not have an accurate “drone” AUV initial position. To further 
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simulate real world conditions, the only data made available to the EKF was the “drone” 


AUV range and heading data and the “master” AUV’s position. 


1. Observability 


In all no dead-reckoning simulations, the rank of the local observability matrix at 
any point in time was four. Unlike the previous scenario, “drone” vehicle position was 
not one of the sensor measurements used to update the EKF. Therefore its states became 
locally unobservable. Using the observability grammian to calculate observability over 
the entire simulation time span, the number of observable states increased to five. 
Initially, the drone's ability to send its heading seems trivial. However, without this data 
it would not be possible for the observability grammian to reach full rank. Figures 4.23 
and 4.24 show the system's local observability and total observability, respectively. 


These figures are applicable to all “master” AUV path radii conducted in this scenario. 


2. ROM Accuracy 


Unlike the simulations with known initial “drone” positions, the EKF did not 
produce accurate estimates in the early stages of the simulations. It generally took one 
“master” AUV cycle before gaining acceptable position data. Initially, the EKF appears 
slow to adjust to the “drone’s” actual position due to their proximity and minimal change 
in range, but once the range and range rate increase the EKF quickly *zeroes in" on the 
drone AUV’s actual position. As demonstrated in the previous model, when the cyclic 
rate of the “master” AUV increased, the accuracy of the EKF also increased. 


Additionally, increasing the “master” AUV’s cyclic rate allowed a faster acquisition of 
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the “drone” by the EKF. They become smaller and recover more quickly as cyclic rate 
increases. This is illustrated by characterizing the initial spikes in the EKF's position 
estimation prior to obtaining an accurate solution in the following figures. Figure 4.25 
compares the actual east-west position to the EKF estimated position for a “master” AUV 
path radius of 100 meters. Figure 4.26 compares the actual north-south position to the 
EKF estimated position for a “master” AUV path radius of 100 meters. Figures 4.27 
through 4.32 show the same information as figures 4.25 and 4.26, for the three other 
“master” AUV paths. The above figures show that as the “master” vehicle”s path radius 
decreases, the initial position error displayed by the spikes lessens and accurate position 
estimates occur quicker. 

Again, ROM target tracking proved to be a more accurate means of position 
estimation than dead reckoning for longer periods of time. For shorter periods of time, 
dead reckoning provided more accurate position estimations. Unlike the previous model, 
there was not a general time under which dead reckoning was more accurate. Since the 
EKF took approximately one cycle to build accurate position solutions, the time in which 
the dead reckoning solution was more accurate varied. 

Once again, decreasing the "master" AUV path radius increased the ет of 
the EKF position estimations. This effect is shown clearly in the following figures. Due 
to the random initial position generated by the program, figures representing any single 
“master” AUV path radius may not be taken from the same simulation. Figure 4.33 
shows the paths of the “master” AUV, and the “drone” AUV”s dead reckoning, 
estimated, and actual paths. Figure 4.34 shows the normalized difference between dead 


reckoning and actual position and the normalized difference between ROM and actual 


positions. Figure 4.35 shows the error between the actual and estimated position states of 
the "drone" AUV. Figures 4.36 through 4.44 illustrate the information displayed in 


Figures 4.33 through 4.35 for the other “master” AUV path radii. 
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Figure 4.1 - Local Observability for all master" AUV path radii 
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Figure 4.2 - Total observability for all “master” AUV path radii 
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Figure 4.3 - East-west EKF position estimates vs. actual position for 100 meter 
*master" AUV path radius 
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Figure 4.4 - North-south EKF position estimates vs. actual position for 100 meter 
*master" AUV path radius 
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Figure 4.5 — East-west EKF position estimates vs. actual position for 50 meter 
*master" AUV path radius 
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Figure 4.6 — North-south EKF position estimates vs. actual position for 50 meter 
*master" AUV path radius 
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Figure 4.7 — East-west EKF position estimates vs. actual position for 30 meter 
*master" AUV path radius 
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Figure 4.8 — North-south EKF position estimates vs. actual position for 30 meter 
“master” AUV path radius 
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Figure 4.9 — East-west EKF position estimates vs. actual position for 17 meter 
“master” AUV path radius 
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Figure 4.10 - North-south EKF position estimates vs. actual position for 17 meter 
“master” AUV path radius 
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Figure 4.11 — AUV paths over 3000 seconds 
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Figure 4.12 - Normalized differences between indicated and actual position for 100 
meter *master" AUV path radius 
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Figure 4.13 — Difference between actual and estimated position states for 100 meter 
“master” AUV path radius 
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Figure 4.14 — AUV paths over 2000 seconds 
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Figure 4.15 — Normalized differences between indicated and actual position for 50 
meter “master” AUV path radius 
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Figure 4.16 — Difference between actual and estimated position states for 50 meter 
*master" AUV path radius 
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Figure 4.17 — AUV paths over 2000 seconds 
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Figure 4.18 — Normalized differences between indicated and actual position for 30 
meter “master” AUV path radius 
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Figure 4.19 — Difference between actual and estimated position states for 30 meter 
“master” AUV path radius 
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Figure 4.20 — AUV paths over 2000 seconds 
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Figure 4.21 — Normalized differences between indicated and actual position for 17 
meter “master’ AUV path radius 
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Figure 4.23 — Local Observability for all master" AUV path radii with no initial 
drone condition data 
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Figure 4.24 — Total observability for all *master" AUV path radii with no initial 
drone condition data 
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Figure 4.25 - East-west EKF position estimates vs. actual position for 100 meter 
*master" AUV path radius with no initial position data 
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Figure 4.26 - North-south EKF position estimates vs. actual position for 100 meter 
*master" AUV path radius with no initial position data 
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Figure 4.27 - East-west EKF position estimates vs. actual position for 50 meter 
*master" AUV path radius with no initial position data 
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Figure 4.28 - North-south EKF position estimates vs. actual position for 50 meter 
“master” AUV path radius with no initial position data 
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Figure 4.29 - East-west EKF position estimates vs. actual position for 30 meter 
*master" AUV path radius with no initial position data 
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ACTUAL vs. ESTIMATED NORTH-SOUTH POSITION 
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Figure 4.30 - North-south EKF position estimates vs. actual position for 30 meter 
*master" AUV path radius with no initial position data 
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ACTUAL vs. ESTIMATED EAST-WEST POSITION 
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Figure 4.31 - East-west EKF position estimates vs. actual position for 17 meter 
“master” AUV path radius with no initial position data 
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ACTUAL vs. ESTIMATED NORTH-SOUTH POSITION 
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Figure 4.32 - North-south EKF position estimates vs. actual position for 17 meter 
*master" AUV path radius with no initial position data. 
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Figure 4.33 - AUV paths with no initial drone position data over 3000 seconds 
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NORMALIZED DIFFERENCES IN POSITION AT TIME (T) 


| | | —— ROM Position Estimate 
550) 1... A E Е. C - Dead Reckoning |... 





0 0.5 1 1.5 2 25% 3 
TIME (SEC) x 10' 


Figure 4.34 — Normalized differences between indicated and actual position for 100 
meter “master” AUV path radius 
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Figure 4.35 — Difference between actual and estimated position states for 100 meter 
“master” AUV path radius 
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Figure 4.36 — AUV paths with no initial drone position data over 2000 seconds 
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Figure 4.37 — Normalized differences between indicated and actual position for 50 
meter ‘‘master’ А ОУ path radius 
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Figure 4.38 — Difference between actual and estimated position states for 50 meter 
*master" AUV path radius 
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Figure 4.39 - AUV paths with no initial drone position data over 2000 seconds 
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Figure 4.40 — Normalized differences between indicated and actual position for 30 
meter “master” AUV path radius 
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Figure 4.41 — Difference between actual and estimated position states for 30 meter 
*master" AUV path radius 
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Figure 4.42 — AUV paths with no initial drone position data over 2000 seconds 
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Figure 4.43 — Normalized differences between indicated and actual position for 17 
meter “master” AUV path radius 
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Figure 4.44 - Difference between actual and estimated position states for 17 meter 
“master” AUV path radius 
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У. CONCLUSIONS AND RECOMMENDATIONS 


A. CONCLUSIONS 


This work, through computer modeling and simulation, has shown that ROM 
position estimation will provide more accurate position estimates than dead reckoning. 
Results from this work also demonstrated that ROM position estimation produces 
observable results. From previous work, it was known that the server path must contain a 
non-zero acceleration motion to enable the EKF to find accurate, observable solutions 
(Song, 1999). However, the results also manifest that the accuracy of the solution is also 
dependent on the tracker vehicle’s path. For the circular paths traveled by the master 
vehicle, larger non-zero accelerations translated to smaller circular paths higher bearing 


and range rates relative to the drone. As these rates increased, accuracy also increased. 


B. RECOMMENDATIONS 


The work and research completed in this thesis utilized simulations that were 
nearly ideal. Based on positive results gathered, further study should be made in both 
development of the EKF and the dynamic model. 

Further optimization of the EKF to reduce the oscillatory errors would greatly 
improve estimated position accuracy. Also testing the EKF’s accuracy when current and 
realistic noise values are modeled could prove very valuable in determining if this system 
could be used on an AUV. 

Future research to enhance the dynamic model should include simulations with 


two or more drones acting independently or simulations with one drone making course 
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changes. Lastly, a vital issue requiring more research is finding alternate master vehicle 


paths that create observable solutions and that patrol operation areas effectively. 
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APPENDIX A. MATLAB CODE ASSOCIATED WITH KNOWN 
INITIAL POSITION SIMULATION 


%Jason Alleyne 

Joinitpos.m 

%2 AUV Range only Measurement Simulation 
% Known Initial Drone Position 


clear 
cle 


%System dynamic model of worker measurement process for the range measurement 


u=1.5; Zoserver vehicle speed, uz1.5 m/s 

Rs=100; 

w-u/Rs; ZoServer vehicle path circular radius, and angular velocity 
psi=0; %the worker vehicle ordered course 

b=(pi/180)*5; the worker vehicle compass bias, 5 degrees 
q=[0.2;0;0]; the worker speed is .2 m/s 

Aw=[0,0,0;0,0,0;0,0,0]; 

x1=zeros(3,2000);x la=zeros(3,2000); Joinitializing the actual (x1a) and dr (x1) position vectors 
х1(:,1)=[0;0;0];х1а(:,1)=[0;0;0]; Zoinitial posits/states at the origin for the simulations. 
speed=zeros(1,2000);compass=zeros(1,2000); initializing the speed and compass vectors 
dil: 7cüme step of one second 

[p2.22]2c2d(A w,q.1); continuous to discrete function for dr course 


for i=1:(length(x1)-1); 
x1(:,1+1)=p2*x1(:,1)+g2; 


speed(1)=g2(1);- %lnitializes speed array for D.R. 
compass(1)=x1(3,1)+b; ZcInitializes compass array for D.R. 
fl(1)=speed(i)*cos(compass(i)); The actual speed/heading due to compass bias 
f2(1)=speed(i)*sin(compass(1)); % The actual speed/heading due to compass bias 
q2=[fl(1,1);f2(1,1);0]; % Used as propagation q matrix 

[p3,g3]=c2d(A w,q2, 1); %C2D to convert continuous actual to discrete actual 
xla(:,i+1)=p3*x1la(:,1)+g3; % Actual track of worker 


V(i)zx1(1,0))-x1a(1,);X(1)2x1(2,1)-x1a(2,1); 
DIFF(i)2sqrt(V (1)^24-X(1)^2); 
end 


circular motion of the server radius, Rs. 
7o xO is the position vector [x,y] of the server 
7o Set up circular path for server. Example is radius of 100 meters at w=0.015 rad/sec. 


%System dynamic model of server measurement process for the range measurement 


As=[0,w;-w,0];q=[1;1); 
xO-zeros(2,2000);x0(:, 1)2[Rs;0]; 
[p,gJ=c2d(As,q, 1); 
for i=1:(length(x0)-1); 
х0(:,1+1)=р*х0(:,1); 
R(i)=sqrt((x0(1,1)-x1(1,1))^2+(x0(2,i)-x 1(2,1))^2); Range between DR worker position and server 
vehicle; 
Ra(i)=sqrt((x0(1,1)-x 1a(1,1))42+(x0(2,1)-x1.a(2,1))*2); %actual Range between worker and server vehicles; 
end 
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figure(2),clf,plot(x0(2,:),x0(1,:), b-5x1(2,:),x1(1,:), g. grid 
xlabel( \fontname{ courier new}\fontsize{12}\bf METERS), 
ylabel( \fontname{ courler new }\fontsize{ 12}\bf METERS); 
title(\fontname {courier new}\fontsize{ 12}\bf AUV PATHS); 
legend( Master AUV Path’, Desired Drone AUV Path), axis equal 


figure(3),clf,plot(R,'g*”), grid 

hold;plot(Ra, b-); 

xlabel( \fontname{ courier new }\fontsize{ 12}\bf TIME (SEC)); 

ylabel( \fontname{ courier new}\fontsize{ 12}\bf RANGE (METERS)); 

title(Afontname [courier new )Montsize[12 bf ACTUAL VS DEAD RECKONING RANGE), 
legend( Dead Reckoning Range, Actual Range 0) 


figure(4),clf plot(x1(2,:),x1(1,:),'8.-x1a(2,:),x1a(1,:),1*x0(2,:),x0(1,:),Db->,grid,axis equal, 
xlabel(AMfontname [courier new)WMontsize[12 bf METERS”); 

ylabel(Montname [courier new }\fontsize{ 12}\bf METERS); 

title( \fontname {courier new}\fontsize{ 12}\bf AUV PATHS); 

legend( Desired Drone AUV Path’,’Actual Drone AUV Path’, Master AUV Path’,0) 


%build an Kalman filter for master vehicle 


% Position Estimator based on data stream, R, 

Io 

endSample=length(Ra);startSample=1; 

y=[Ra;x0(1,1:length(Ra));x0(2, 1:length(Ra));x1a(1,1:length(Ra));x1a(2,1:length(Ra));x1a(3,1:length(Ra))]; 
x=zeros(5,endSample);err=zeros(6,endSample); 

s=startS ample; 

x(:,s)=[x0(1,1),x0(2,1),0,0,0]’; 


%MANEUVERING AND CURRENT TIME CONSTANTS 


tau120; 
tau2=0; 


A=[As,zeros(2,3);zeros(3,2),Aw]; 


C=zeros(6,5); JoInitializes the C matrix 
О ЕЕ = СЗ )Е КС (БАЈЕ КС(б э)= % Matches measurement states (y) to output states (x) 


Ra(1)2sqrt((x0(1,1)-x1a(1,1))^24(x0(2,1)-x1a(2,1))^2); 


Zo C matrix local is dg/dx 

C(1,3)2-(xO(1, 1)-x1a(1, 1) /Ra(1); 
C(1,4)2-(x0(2,1)-x1a(2, D)/Ra(1); 
C(1,1)2(x0(1,1)-x1a(1, 1D) Ra(1); 
C(1,2)-(x0(2,1)-x1a(2, 1D)/Ra(1); 


%Initial B matrix 


gl=0 01; %variance on X, m^2 
q2=0.01; Zovariance on Y, m^2 
q3=0.1; Фоуапапсе оп х1а 

94<.1; Zovariance on у!а, rad/s)^2 
q5=0.0001; Zoslow bias convergence 
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В=[41;42;43;94;95]/100; 

Qzdiag(B); Zosystem noise 
nul=0.01;nu2=.001 ;nu3=.001 ;nu4=2000;nuS=2000;nu6=.02; 
gnu-[nul;nu2;nu3;nu4;nu5;nu6]; 


R=diag(gnu)* 1; Zomeasurement noise 
Gram=zeros(5,5); Zeiniuialization of observability grammian matrix 


% measured data at old time, new. before means model predicted value 


_old_after=eye(5); 
delx_old_after=zeros(5,1); 


g=ones(6, 1);psave=zeros(5,length(R)); 


dt=1;phi=expm(A *dt); t=0:1:endSample-1; 


for i=startSample:(endSample-1); 


Ycompute linearized PHI matrix using updated A 
Zoreset initial state 
xsoldeafter xt). 
% nonlinear state propagation 
[x_new_before]=phi*x_old_after; 
Фреггог соуапапсе ргорагапоп 
p_new_before=phi*p_old_after*phi’+ Q; new gain calculation using linearized new 
%C matrix and current state error 


Фосоуапапсез. 


Zoformulate the innovation using nonlinear output propagation 
%as compared to new sampled data from measurements 


yhat-output3(x new. before); 
err(:,1+1 )=(y(:,1+1) - yhat); 


G=p_new_before*C *inv(C*p_new_before*C’ + R); % Kalman Gain 
p_old_after=[eye(5) - G*C]*p_new_before; 
psave(:,i+1)=diag(p_old_after); 


cpc=inv(C*p_old_after*C’+R); 
rel(i+1)=err(:,1+1)*cpc*err(:,1+1); 


x_new_after=x_new_before + G*err(:,1+1); 
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Jocarry new state into next update 
x(:,1+1)=x_new_after; 


C=zeros(6,5); 
оше ЗОО) - ЦС 3)- СО 22 С16.5)-5 Current settings make all target states observable to 
%НИег. 


7o Cmatrix local is dg/dx 


C(1,3)2-(x(1,)-x3,))/y (1,1); 
C(1,4)2-(x(2.D-x(4,1) V y (1,1); 

COE Ds-C(1,3); 

С(1,2)--С(1,4); 

O(i)-zrank((C'phi *C ]); 
О(1)=х(4,1)-у(5,1);Т(1)=х(3,1)-у(4,1); 
01#6(1)=59г100(1)^2+7(1)^2); 
GramzGram-phi *C *inv(R)*C*phi; 
Gramrank(1)=rank(Gram); 

end: 


figure(5),clf,plot(O,b*”,grid,zoom %Local Observability indicates # of observable states at a point in 
Фите. 

xlabel( \fontname{ courier new}\fontsize{ 12}\bf TIME (SEC)); 

ylabel( \fontname{ courier new}\fontsize{ 12}\bf OBSERVABLE STATES); 

title( \fontname {courier new} \fontsize{12}\bf LOCAL SYSTEM OBSERVABILITY) 


figure(6),clf,plot(Gramrank, b*?),grid,zoom %Observability Grammian Rank indicates # of observable 
Jostates over period of the simulation 

xlabel( \fontname{ courier new }\fontsize{ 12}\bf TIME (SEC)); 

ylabel( \fontname{ courier new}\fontsize{ 12}\bf OBSERVABLE STATES); 

title(\fontname{ courier new}\fontsize{ 12}\bf TOTAL SYSTEM OBSERVABILITY 9) 


figure(7),clf,plot(t,rel, b*^,grid,zoom 


figure(8),clf,plot(t,x(4,:),b*”t,x1a(2,1:1999),'m.”, grid JoEast-West position comparison 
xlabel( \fontname{ courier new }\fontsize{ 12}\bf TIME (SEC)); 

ylabel( \fontname{ courier new}\fontsize{ 12}\bf METERS); 

title(\fontname{ courier new}\fontsize{ 12}\bf ACTUAL vs. ESTIMATED EAST-WEST POSITION); 


, 


legend( Estimated position’,’Actual position 0) 


figure (9),clf,plot(t,x(3,:), b*,t,x1a(1,1:1999),'m.’),grid YNorth-south position comparison 

xlabel( \fontname{ courier new }\fontsize{ 12}\bf TIME (SEC)); 

ylabel( \fontname{ courier new}\fontsize{ 12}\bf METERS); 

title(\fontname{ courier new}\fontsize{ 12}\bf ACTUAL vs. ESTIMATED NORTH-SOUTH POSITION); 
legend( Estimated position’, Actual position’,0) 


figure(10),clf,plot(x(2,:),x(1,:)),grıd,hold, 
plot(x(4,:),x(3,:),T.),plot(x1(2,:),x1(1,:),'m.),plot(x1a(2,1:1999),x1a(1,1:1999),'c.”);zoom,hold off,axis 
equal 

xlabel(\fontname{ courier new }\fontsize{ 12}\bf METERS); 

ylabel( \fontname {courier new} \fontsize{ 12}\bf METERS); 

title( \fontname {courier new }\fontsize{ 12}\bf AUV PATHS); 

legend( Master AUV Path’, Estimated Drone Path’, Desired Drone AUV Path’,’Actual Drone AUV Path’,0) 
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figure(11),clf,plot(err”, grid, 

xlabel( Montname (courier new JMontsize( 12] bf TIME (SEC)); 

ylabel( \fontname {courier new }\fontsize{ 12}\bf METERS); 

title( \fontname{ courier new }\fontsize{12}\bf DIFFERENCE BETWEEN ACTUAL AND ESTIMATED 


VALUES): 
legend(’Range’, Server North-South’, Server East-West’, Worker North-South’, Worker East-West’, Worker 


Heading’,O) 


figure(12),clf,plot(diff, r-’),grid,hold,plot(DIFF, b.’),hold off 

xlabel(\fontname {courier new}\fontsize{ 12}\bf TIME (SEC); 

ylabel( \fontname {courier new} \fontsize{ 12}\bf METERS); 

title( \fontname {courier new }\fontsize{12}\of NORMALIZED DIFFERENCES IN POSITION AT TIME 
(Т)); 

legend(ROM Position Estimate” Dead Reckoning',0) 
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APPENDIX B. MATLAB CODE ASSOCIATED WITH UNKNOWN 
INITIAL POSITION SIMULATION 


%Jason Alleyne 

Jono-initpos.m 

%2 AUV Range only Measurement Simulation 
% Unknown Initial Drone Position 


clear 
cle 


%System dynamic model of worker measurement process for the range measurement 


u=1.5; server vehicle speed, u=1.5 m/s 

Rs=100;w=u/Rs; Server vehicle path circular radius, and angular velocity 

psi=0; the worker vehicle ordered course 

b=(pi/180)*5; %the worker vehicle compass bias, 5 degrees 

q=[0.2;0;0]; %іһе worker speed is .2 m/s 

Aw=[0,0,0;0,0,0;0,0,0]; 

xl=zeros(3,2000);x la=zeros(3,2000); Zoinitializing the actual (xla) and dr (x1) position vectors 


х1(:,1)=[0;0;:0] 1=гапап([2,1])*В $; х1а(:,1)=[1;0]; 2b The actual position is unknown, and is a random 
initial value. 


speed=zeros(1,2000);compass=zeros(1,2000); Zoinitializing the speed and compass vectors 
dt Zotime step of one second 
[p2,22]=c2d(Aw,q, 1); continuous to discrete function for dr course 


for ı=1:(length(x1)-1); 
x1C,i*1)2p2*x1(:,1)4g2; 


speed(i)=g2(1); %lnitializes speed array for D.R. 
compass(i)=x1(3,1)+b; %lnitializes compass array for О.К. 
fl(1)=speed(i)*cos(compass(ı)); 915 the actual speed/heading due to compass bias 
f2(1)2speed(i)*sin(compass(1)); 915 the actual speed/heading due to compass bias 
q2=[f1(1,1);f2(1,1);0]; Zbused as propagation q matrix 

[p3,23]2c2d(A w,q2,1); %C2D to convert continuous actual to discrete actual 
x 1la(:,1+1)=p3*x la(:,1)+g3; %actual track of worker 


V(N=x10,0-x1a(1,0;X(0)=x1(2,1)-x1a(2,1); 
DIFF(i)2sqrt(V (1)^24-X(1)^2); 
end 


Zocircular motion of the server radius, Rs. 
% x0 is the position vector [x,y] of the server 
% Set up circular path for server. Example is radius of 100 meteres at w=0.015 rad/sec. 


%System dynamic model of server measurement process for the range measurement 


As=[0,w;-w,0];q=[1;1]; 
x0=zeros(2,2000);x0(:,1)=[Rs;0]; 
[р,5]=с24(А$,4,1); 
for 1=1:(length(x0)-1); 
x0(:,1+1)=p*x0( 0,1); 
В(1)=ѕаг((х0(1,1)-х1(1,1))^2+(х0(2 1)-х1(21))^2);  ZRange between estimated worker position and 
server vehicle; 
Ra(i)=sqrt((x0(1,1)-xla(1,1)^2+(x0(2,i)-xla(2,1))^2); %actual Range between worker and server vehicles 
end 
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figure l),clf,plot(x0(2,:),xX0(1,:), b-x1(2,:),x1(1,:),g.). grid 
xlabel(Montname ( courier new}\fontsize{ 12}\bf METERS); 
ylabel( \fontname {courier new}\fontsize{12}\bf METERS); 
title(\fontname{ courier new}\fontsize{ 12}\bf AUV PATHS); 
legend( Master AUV Path’, Desired Drone AUV Path’, axis equal 


figure(2),clf,plot(R, ¢*’),grid 

hold;plot(Ra, b->; 

xlabel( \fontname{ courier new}\fontsize{ 12}\bf TIME (SEC)); 

ylabel( \fontname{ courier new}\fontsize { 12}\bf RANGE (METERS)); 

title( \fontname {courier new }\fontsize{ 12}\bf ACTUAL VS DEAD RECKONING RANGE’; 
legend( Dead Reckoning Range’,’Actual Range’,0) 


heures wel ро(х1(2 ) хр) AAA IO grid axs equal, 
xlabel( \fontname {courier new}\fontsize{ 12}\bf METERS); 

ylabel(\fontname {courier new}\fontsize{ 12}\bf METERS); 

title( \fontname{ courier new}\fontsize{ 12}\bf AUV PATHS); 

legend( Desired Drone AUV Path’,’Actual Drone AUV Path’, Master AUV Path’,0) 


%build a Kalman filter for server vehicle 


9o Position Estimator based on data stream, R, 

% 

endSample=length(Ra);startSample=1; 

y=[Ra;x0(1,1:length(Ra));x0(2, 1 :length(Ra));x 1a(1, 1:length(Ra));x 1a(2,1:length(Ra));x 1 a(3, 1 :length(Ra))]; 
x=zeros(5,endSample);err=zeros(6,endSample); 

s=startSample; 

= ОИ хо 150: 0]5 


%MANEUVERING AND CURRENT TIME CONSTANTS 


taul=0; 
tau2=0; 


A=[As,zeros(2,3);zeros(3,2),Aw]; 


C=zeros(6,5); %Initializes the C matrix 
C 2 |): C(32)=1:C(4.3)=0:C(5.4)<X0:C(6 5)=0; % Matches measurement states (y) to output states (x) 
with drone states unobservable 


Ка(1)=заг((х0(1,1)-х1а(1,1))^2+(х0(2,1)-х1а(2,1))^2); 


7o Cmatrix local is dg/dx 

C(1,3)2-(x0(1,1)-x1a(1,1))/Ra(1); 
С(1,4)+-(х0(2,1)-х1а(2,1)УКа(1); 
C(1,1)2(x0(1,1)-x1a(1,1))/Ra(1); 
C(1,2)2(x0(2,1)-x1a(2,1))/Ra(1); 


% Initial B matrix 


ШЕШ Фоуапапсе on X, m^2 
q2=0.01; %уапапсе оп У, т^2 
q3=0.1; Jevariance on xla 

а= Фоуапапсе оп yla, rad/s)^2 
q5=0.0001; %slow bias convergence 
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В-(41;42;43;44:451/100; 

Q=diag(B); 

Josystem noise 

nul=0.01;nu2=.001 ;nu3=.001 ;nu4=2000;nuS=2000;nu6=.02; 
gnu-[nul;nu2;nu3;nu4;nu5;nu6]; 


R=diag(gnu)* 1; measurement noise 
Gram=zeros(5,5); initialization of observability grammian matrix 


% measured data at old time, new. before means model predicted value 


p_old_after=eye(5); 
delx_old_after=zeros(5,1); 


g=ones(6,1);psave=zeros(5,length(R)); 
dt=1;phi=expm(A*dt); t=0:1:endSample-1; 
for i=startSample:(endSample-1); 
%compute linearized PHI matrix using updated A 
reset initial state 
x_old_after=x(:,1); 
% nonlinear state propagation 
[x_new_before ]=phi*x_old_after; 
Yoerror covariance propagation 
p_new_before=phi*p_old_after*phi’ + Q; Yonew gain calculation using 


Jolinearized new C matrix and 
%current State error covariances. 


formulate the innovation using nonlinear output propagation 
Yoas compared to new sampled data from measurements 


yhat=output3(x_new_before); 

еп(:,1+1)=(у(:,1+1) - yhat); 

G-p. new before*C'*inv(C*p new. before*C"' + К); Фо Kalman Gain 
p.old afterz[eye(5) - G*C]*p. new. before; 


psave(:,i*- 1)2diag(p. old after); 


cpczinv(C*p. old, after*C-R); 
rel(i+1)=err(:,i+1)*cpc*err(:,i+1); 


x_new_after=x_new_before + G*err(:,1+1); 
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Zocarry new state into next update 


xC,i-1)zx, new. after; 


Czzeros(6,5); 
Cea —1:603:25—1:C(4,3)20;C(5,4)20:C(6,5) 51; 


Zo Cmatrix local is dg/dx 


C(1,3)2-(x(1,))-x(31))/ y (1,1); 
C(1,4)2-(xQ.)-x(4,1))/y(1,1); 

CODI ECCL): 

С(1,2)=-С(1.4); 

а рис 
О(1)=х(4,1)-у(5,1);Т(1)=х(3 1)-у(4 1); 
diff(i)2sqrt(U(1)^2* T(1)^2); 
Gram-Gram-*phi *C *inv(R)*C*phi; 
Gramrank(i)=rank(Gram); 

end; 


figure(4),clf,plot(O, b*), grid JoLocal Observability indicates # of observable states at a point in time. 
xlabel( \fontname{ courier new }\fontsize{ 12}\bf TIME (SEC); 

ylabel( \fontname{ courier new}\fontsize{ 12 }\bf OBSERVABLE STATES); 

title(\fontname {courier new }\fontsize{ 12}\bf LOCAL SYSTEM OBSERVABILITY ) 


figure(5),clf,plot(Gramrank, b *),grid,axis([O length(x0) 4 6]) %Observability Grammian Rank indicates 
%of observable states over period of the 
%simulation 

xlabel( \fontname{ courier new }\fontsize{ 12 }\bf TIME (SEC); 

ylabel( \fontname {courier new}\fontsize{ 12 }\bf OBSERVABLE STATES); 

title( \fontname {courier new }\fontsize{ 12}\bf TOTAL SYSTEM OBSERVABILITY ) 


figure(6),clf,plot(t,rel, b*’),grid,zoom 


figure(7),clf plot(t,x(4,:),b*”t,x1a(2,1:1999),'m.”,grid Zo East- West position comparison 
xlabel( \fontname{ courier new }\fontsize{ 12}\bf TIME (SEC); 

ylabel( \fontname {courier new }\fontsize{ 12}\bf METERS); 

title( \fontname{ courier new}\fontsize{ 12}\bf ACTUAL vs. ESTIMATED EAST-WEST POSITION), 
legend( Estimated position’,’Actual position’,O) 


figure(8),clf,plot(t,x(3,:), b*’,t,x 1a(1,1:1999),'m.?, grid %North-south position comparison 

xlabel( Montname (courier new }\fontsıze{ 12 }\bf TIME (SECY); 

ylabel( \fontname {courier new }\fontsize{ 12}\bf METERS); 

title(\fontname {courier new }\fontsize{ 12}\bf ACTUAL vs. ESTIMATED NORTH-SOUTH POSITION); 
legend( Estimated position’, Actual position’,O) 


figure(9),clf,plot(x(2,:),x(1,:)), grid, hold, 
plot(x(4,:),x(3,:),T.),plot(x1(2,:),x1(1,:),'m.),plot(x1a(2,1:1999),x1a(1,1:1999),'c.”;hold off,axis 
equal,zoom 

xlabel( \fontname{ courier new}\fontsize{ 12}\bf METERS); 

ylabel(\fontname { courier new }\fontsize{ 12 }\bf METERS); 

title( \fontname {courier new }\fontsize{ 12 }\bf AUV PATHS); 

legend( Master AUV Path’, Estimated Drone Path’, Desired Drone AUV Path’,’Actual Drone AUV Path’,O) 
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figure(10),clf,plot(err),grid, 

xlabel( \fontname {courier new}\fontsize{ 12}\bf TIME (SEC)); 

ylabel( \fontname{ courier new}\fontsize{ 12}\bf METERS); 

title(\fontname {courier new}\fontsize{ 12}\bf DIFFERENCE BETWEEN ACTUAL AND ESTIMATED 
VALUES); 

legend( Range’, Server North-South’, Server East-West’, Worker North-South’, Worker East-West’, Worker 


Heading',0) 


figure(11),clf,plot(diff, r-),grid,hold,plot(DIFF, b.^,hold off 

xlabel(Montname (courier new Montsize( 12) Vf TIME (SEC)); 

ylabel(Montname (courier new)Montsize( 12) bf METERS); 

title( \fontname{ courier new}\fontsize{ 12}\bf NORMALIZED DIFFERENCES IN POSITION AT TIME 
(Т)); 

legend( ROM Position Estimate Dead Reckoning 0) 
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APPENDIX C. MATLAB CODE ASSOCIATED WITH EXTENDED 
KALMAN FILTER MEASUREMENT DATA FUSION 


%Jason Alleyne 
JOutput3.m 
J%Function required for ROM EKF used in initpos.m and no-initpos.m 


function [yhat]=output3(xold); 


x0=xold(2); 
yO=xold(1); 
x1=xold(4); 
y1=xold(3); 
psil=xold(5); | %ensure the states are in the same order as in main program 


Rhat=sqrt((x0-x 1)42+(y0-y1)%2); 
xOhat=x0; 
yOhat=y0; 


yhat=[Rhat;y0;x0;y1;x1;ps11]; 
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